/*
 * Copyright (c) 2011-2014, fortiss GmbH.
 * Licensed under the Apache License, Version 2.0.
 *
 * Use, modification and distribution are subject to the terms specified
 * in the accompanying license file LICENSE.txt located at the root directory
 * of this software distribution. A copy is available at
 * http://chromosome.fortiss.org/.
 *
 * This file is part of CHROMOSOME.
 *
 * $Id: executeROSFunction.cpp 7805 2014-03-13 09:54:35Z geisinger $
 */

/**
 * \file
 *         Source file for function executeROS in component ROS.
 *
 * \author
 *         This file has been generated by the CHROMOSOME Modeling Tool (XMT)
 *         (fortiss GmbH).
 */

/******************************************************************************/
/***   Includes                                                             ***/
/******************************************************************************/
#include "rOSGateway/adv/rOS/include/executeROSFunction.h"

#include "rOSGateway/adv/rOS/include/executeROSFunctionWrapper.h"
#include "rOSGateway/adv/rOS/include/rOSComponent.h"
#include "rOSGateway/adv/rOS/include/rOSComponentWrapper.h"
#include "rOSGateway/adv/rOS/include/rOSManifest.h"

#include "xme/core/logUtils.h"

// PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_C_INCLUDES) ENABLED START
#include "xme/defines.h"
#include "xme/hal/include/ros.h" //TODO
#include "xme/hal/include/sched.h" //TODO
#include "xme/hal/include/time.h" //TODO
// PROTECTED REGION END

/******************************************************************************/
/***   Definitions                                                          ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_C_DEFINITIONS) ENABLED START
// PROTECTED REGION END

/******************************************************************************/
/***   Variables                                                            ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_C_VARIABLES) ENABLED START
static xme_hal_sched_taskHandle_t rosTaskHandle = XME_HAL_SCHED_INVALID_TASK_HANDLE;
// PROTECTED REGION END

/******************************************************************************/
/***   Prototypes                                                           ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_C_PROTOTYPES) ENABLED START
static void runROSTaskCallback(void* userData);
// PROTECTED REGION END

/******************************************************************************/
/***   Implementation                                                       ***/
/******************************************************************************/
xme_status_t
rOSGateway_adv_rOS_executeROSFunction_init
(
    rOSGateway_adv_rOS_rOSComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_INITIALIZE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);

	xme_hal_ros_init();

	rosTaskHandle = xme_hal_sched_addTask(xme_hal_time_timeIntervalFromMilliseconds(0), xme_hal_time_timeIntervalFromMilliseconds(50), 0, runROSTaskCallback, NULL);
	if (XME_HAL_SCHED_INVALID_TASK_HANDLE == rosTaskHandle)
	{
		XME_EXIT(EXIT_FAILURE, "Error creating runROSThread\n");
	}

    return XME_STATUS_SUCCESS;
    // PROTECTED REGION END
}

void
rOSGateway_adv_rOS_executeROSFunction_step
(
    rOSGateway_adv_rOS_rOSComponent_config_t* const componentConfig
)
{
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_STEP_C) ENABLED START
        XME_UNUSED_PARAMETER(componentConfig);
        // PROTECTED REGION END
    }
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_STEP_2_C) ENABLED START
        // PROTECTED REGION END
    }
}

void
rOSGateway_adv_rOS_executeROSFunction_fini
(
    rOSGateway_adv_rOS_rOSComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_TERMINATE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);
    
	xme_hal_sched_removeTask(rosTaskHandle);
	xme_hal_ros_fini();
    // PROTECTED REGION END
}

// PROTECTED REGION ID(ROSGATEWAY_ADV_ROS_EXECUTEROSFUNCTION_IMPLEMENTATION_C) ENABLED START
static void runROSTaskCallback(void* userData)
{
	XME_UNUSED_PARAMETER(userData);

	xme_hal_ros_spin();

	XME_EXIT(EXIT_FAILURE, "ROS has been shut-down!\n");
}
// PROTECTED REGION END
